#ifndef MERGED_CLOUD_HPP
#define MERGED_CLOUD_HPP

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <mutex>
#include <deque>
#include <numeric>

/**
 * @brief 优化后的点云拼接类，采用增量更新策略提高效率
 */
class Merged_cloud {
public:
    /**
     * @brief 构造函数
     * @param max_frames 最大存储帧数，默认为5
     */
    Merged_cloud(size_t max_frames = 5) : max_frames_(max_frames) {
        merged_cloud_ = pcl::PointCloud<pcl::PointXYZI>::Ptr(new pcl::PointCloud<pcl::PointXYZI>());
    }

    ~Merged_cloud() = default;

    /**
     * @brief 添加新的点云到拼接队列，采用增量更新策略
     * @param laser_cloud 待添加的点云指针
     */
    void add_cloud(pcl::PointCloud<pcl::PointXYZI>::Ptr laser_cloud) {
        if (!laser_cloud || laser_cloud->empty()) {
            return; // 忽略空点云
        }

        std::lock_guard<std::mutex> lock(mutex_);
        
        // 记录当前帧的点数量，用于后续快速计算
        size_t point_count = laser_cloud->size();
        
        // 添加新帧
        cloud_frames_.push_back(laser_cloud);
        frame_point_counts_.push_back(point_count);
        
        // 增量添加新点云，避免重复合并所有帧
        *merged_cloud_ += *laser_cloud;
        
        // 如果超过最大帧数，删除最早的一帧并更新合并结果
        if (cloud_frames_.size() > max_frames_) {
            // 移除最早帧的点
            size_t oldest_count = frame_point_counts_.front();
            if (oldest_count <= merged_cloud_->size()) {
                // 直接删除最早帧的点，避免重新合并所有帧
                merged_cloud_->erase(merged_cloud_->begin(), 
                                    merged_cloud_->begin() + oldest_count);
            } else {
                // 异常情况，重新合并所有帧
                mergeAllFrames();
            }
            
            // 移除最早的帧和其点计数
            cloud_frames_.pop_front();
            frame_point_counts_.pop_front();
        }
    }

    /**
     * @brief 获取当前存储的点云帧数
     * @return 点云帧数
     */
    size_t get_fram_sum() const {
        std::lock_guard<std::mutex> lock(mutex_);
        return cloud_frames_.size();
    }

    /**
     * @brief 获取拼接后的点云指针
     * @return 拼接后的点云指针
     */
    pcl::PointCloud<pcl::PointXYZI>::Ptr get_merged_cloud() const {
        std::lock_guard<std::mutex> lock(mutex_);
        // 返回共享指针的副本，避免外部修改影响内部状态
        return merged_cloud_;
    }

    /**
     * @brief 清空所有点云和计数
     */
    void clear() {
        std::lock_guard<std::mutex> lock(mutex_);
        cloud_frames_.clear();
        frame_point_counts_.clear();
        merged_cloud_->clear();
    }

    /**
     * @brief 设置最大存储帧数
     * @param max_frames 新的最大帧数限制
     */
    void set_max_frames(size_t max_frames) {
        if (max_frames == 0) return; // 避免无效值
        
        std::lock_guard<std::mutex> lock(mutex_);
        max_frames_ = max_frames;
        
        // 如果当前帧数超过新的最大值，删除多余的最早帧
        while (cloud_frames_.size() > max_frames_) {
            size_t oldest_count = frame_point_counts_.front();
            if (oldest_count <= merged_cloud_->size()) {
                merged_cloud_->erase(merged_cloud_->begin(), 
                                    merged_cloud_->begin() + oldest_count);
            }
            
            cloud_frames_.pop_front();
            frame_point_counts_.pop_front();
        }
        
        // 安全检查：如果删除过程中出现异常，重新合并所有帧
        size_t total_points = std::accumulate(frame_point_counts_.begin(), 
                                             frame_point_counts_.end(), 0);
        if (merged_cloud_->size() != total_points) {
            mergeAllFrames();
        }
    }

private:
    /**
     * @brief 重新合并所有保留的点云帧（仅在必要时使用）
     */
    void mergeAllFrames() {
        merged_cloud_->clear();
        merged_cloud_->reserve(std::accumulate(frame_point_counts_.begin(), 
                                             frame_point_counts_.end(), 0));
        
        for (const auto& frame : cloud_frames_) {
            *merged_cloud_ += *frame;
        }
    }

    mutable std::mutex mutex_;                          // 线程安全锁
    size_t max_frames_;                                 // 最大存储帧数
    pcl::PointCloud<pcl::PointXYZI>::Ptr merged_cloud_;  // 拼接后的点云
    std::deque<pcl::PointCloud<pcl::PointXYZI>::Ptr> cloud_frames_;  // 存储原始帧
    std::deque<size_t> frame_point_counts_;             // 记录每个帧的点数量，用于快速删除
};

#endif // MERGED_CLOUD_HPP
    